#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/ioctl.h>
#include <sys/stat.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>

#include "bmi088.h"

/**
******************************************************************************
* @file           : bmi088.c
* @brief          : implement for bmi088.c file.
*                   This file contains the common implement of the application.
* @author			: ac
* @attention
*
*		Copyright (c) 2021 Technology Co., Ltd.
*
*******************************************************************************
*/

/*
 * globla variable
 */



/*
 * function declaration
 */
int8_t user_spi_read(uint8_t reg_addr, uint8_t *data, uint16_t len);

/**
 * icm_spi_open - open sip device.
 * @none
 */
static XAG_U32 icm_spi_open(void)
{
    XAG_U32 fd = 0xac;

    fd = open(SPI00, O_RDWR);
	if (fd < 0) {
		__do_abnormal("spi open");
	}
	else 
		printf("spi open succeed.\n");

    return fd;
}

/**
 * icm_spi_config - sip device config.
 * @fd : spi device number
 */
static XAG_U32 icm_spi_config(XAG_U32 fd)
{
	XAG_U8 mode = 0, bits = 8;
	XAG_U32 speed = 5 * 1000 * 1000;	/* 24M */
	XAG_U32 ret = 0xac;

	/* spi mode */
	ret = ioctl(fd, SPI_IOC_WR_MODE, &mode);
	if (ret == -1) 
		__do_abnormal("spi wr mode");

	ret = ioctl(fd, SPI_IOC_RD_MODE, &mode);
	if (ret == -1)
		__do_abnormal("spi rd mode");

	/* bits per word */
	ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
	if (ret == -1)
		__do_abnormal("spi set bits");

	ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);
	if (ret == -1)
		__do_abnormal("spi get bits");

	/* max speed hz */
	ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
	if (ret == -1)
		__do_abnormal("spi set max speed");

	ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);
	if (ret == -1)
		__do_abnormal("spi get max speed");

	printf("spi mode: %d\n", mode);
	printf("bits per word: %d\n", bits);
	printf("max speed: %d KHz (%d MHz)\n", speed / 1000, speed / 1000 / 1000);

	return 0;
}

int readfrom_spi_serial
(	
	XAG_U32       fd,
    XAG_U16       headerLength,
    const XAG_U8 *headerBuffer,
    XAG_U32       readlength,
    XAG_U8       *readBuffer
)
{
	unsigned char  buf[128] = {0};
	unsigned char  rx_buf[128] = {0};
	struct spi_ioc_transfer mesg[1];
	int retval = 0;
	int i = 0, index = 0;

	memset(mesg, 0, sizeof(struct spi_ioc_transfer));

	mesg[0].tx_buf = (__u32)buf;
	mesg[0].rx_buf = (__u32)rx_buf;
	mesg[0].len = headerLength + readlength;
	mesg[0].speed_hz = 5000000;
	mesg[0].bits_per_word = 8;
	mesg[0].delay_usecs = 10000;

	//*(unsigned char  *)(&buf[0]) = headerBuffer[0];
	//*(unsigned char  *)(&buf[1]) = headerBuffer[1];

	for (index = 0; index < headerLength; index++) {
		*(unsigned char  *)(&buf[index]) = headerBuffer[index];
	}

	#if 1
	for (i = 0; i < headerLength; i++) {
		printf("buf = 0x%x\n", *(unsigned char *)(&buf[i]));
		printf("headerBuffer = 0x%x\n", headerBuffer[i]);
	}
	#endif

	retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
	//printf("retval = %d\n", retval);
	if (retval < 1) 
		printf("can't send spi message"); 

	printf("read buf:");
	for (i = 0; i < 0x10; i++) {
		printf(" 0x%x", *(unsigned char *)(&rx_buf[i]));
	}
	printf("\n");

	for (i = headerLength; i < readlength + headerLength; i++) {
		printf("read result = 0x%x\n", *(unsigned char *)(&rx_buf[i]));
		readBuffer[i-headerLength] = *(unsigned char *)(&rx_buf[i]);
	}

    return 0;
}

int dwt_readfromdevice
(
		XAG_U32       fd,
    unsigned char  recordNumber,
    unsigned char  index,
    unsigned long   length,
    unsigned char   *buffer
)
{
    unsigned char header[3] = {0} ;                                       
    unsigned short   cnt = 0;                                          

	printf("dwt_readfromdevice recordNumber = 0x%x\n", recordNumber);
    if (index == 0)                                         // for index of 0, no sub-index is required
    {
        header[cnt++] = (unsigned char) recordNumber ;              // bit-7 zero is READ operation, bit-6 zero=NO sub-addressing, bits 5-0 is reg file id
    }
    else
    {
        header[cnt++] = (unsigned char)(0x40 | recordNumber) ;      // bit-7 zero is READ operation, bit-6 one=sub-address follows, bits 5-0 is reg file id

        if (index <= 127)                                   // for non-zero index < 127, just a single sub-index byte is required
        {
            header[cnt++] = (unsigned char) index ;                 // bit-7 zero means no extension, bits 6-0 is index.
        }
        else
        {
            header[cnt++] = 0x80 | (unsigned char)(index) ;           // bit-7 one means extended index, bits 6-0 is low seven bits of index.
            header[cnt++] =  (unsigned char) (index >> 7) ;           // 8-bit value = high eight bits of index.
        }
    }

	printf("cnt = %d, 0x%x 0x%x 0x%x\n", cnt, header[0], header[1], header[2]);

    return readfrom_spi_serial(fd, cnt, header, length, buffer);  // result is stored in the buffer
}

/**
 * imu_spi_read_bmi088_id - read bmi088 id.
 * @fd : spi device number
 */
void imu_spi_read_bmi088_id(XAG_U32 fd)
{
	unsigned char readBuffer[8] = {0};
	unsigned char i , readlength;

	readlength = 1;
	dwt_readfromdevice(fd, 0x00|0x80, 0x00, readlength, readBuffer);

	for (i = 0; i < readlength; i++) {
		printf("bmi088 id = 0x%x\n", readBuffer[i]);
	}
}


/**
 * icm_spi_close - close sip device.
 * @fd : spi device number
 */
static XAG_U32 icm_spi_close(XAG_U32 fd)
{
	if (fd < 0) {
		__do_abnormal("spi close");
	}
	
	close(fd);

	return 0;
}


/**
 * __do_abnormal - exception handling.
 *	@str : function name
 */
void __do_abnormal(const char * str)
{
    printf("%s abnormal, program terminated...\n", str);
    exit(0);
}

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))  

static void transfer()                                                    
{                                                                               
        int ret;   
		int fd;
		int tmp = 0;

        uint8_t tx[2] = {                                                        
                0x00|0x80,                                      
        };       
				
        uint8_t rx[ARRAY_SIZE(tx)] = {0, };                                     
 
        struct spi_ioc_transfer tr[1];     
		
		fd = open("/dev/spidev0.0", O_RDWR);
		if (fd < 0)
		{
			printf("failed to open spidev0.0\n");
		}

		tmp = SPI_MODE_3; //  | SPI_LSB_FIRST;

		ret = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
		if (ret)
		{
			printf("set spi mode fail!\n");
			return -1;
		}

		memset(tr, 0, sizeof(struct spi_ioc_transfer));		// 没有这行会报 can't send spi message 的错。

        tr[0].tx_buf = (unsigned long)tx;
        tr[0].rx_buf = (unsigned long)rx;                                       
        tr[0].len = ARRAY_SIZE(tx);                                             
        tr[0].speed_hz = 5000000;                                                 
        tr[0].bits_per_word = 8;                                             
        tr[0].delay_usecs = 1000;
        tr[0].cs_change = 1;
                                                            
        ret = ioctl(fd, SPI_IOC_MESSAGE(1), tr);                                
        if (ret < 1)                                                            
                printf("can't send spi message");                               
 
        for (ret = 0; ret < ARRAY_SIZE(tx); ret++) {                            
                if (!(ret % 6))                                                 
                        puts("");                                               
                printf("0x%.2X ", rx[ret]);       // 0x00 0x0F                                 
        }                                                                       
        puts("");   
		
		close(fd);
}   

int user_spi_readGRY(uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	int fd;
	int ret = -1;
	int tmp = 0;
	unsigned char buf[0x10] = {0};
	struct spi_ioc_transfer tr[1];
   
	fd = open("/dev/spidev0.0", O_RDWR);
	if (fd < 0)
	{
		printf("failed to open spidev0.0\n");
	}

	tmp = SPI_MODE_3; //  | SPI_LSB_FIRST;

	ret = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (ret)
	{
		printf("set spi mode fail!\n");
		ret = -1;
		goto end_spi_read;
	}
	
	memset(tr, 0, sizeof tr);
	tr[0].tx_buf = (__u64)buf;
	tr[0].rx_buf = (__u64)buf;
	tr[0].len = 2; 
	tr[0].speed_hz = 1500000;
	tr[0].bits_per_word = 8;
	tr[0].cs_change = 1;
    
	buf[0] = reg_addr;
	for (size_t i = 0; i < len; i++)
	{
		buf[0] = (reg_addr + i);
		ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
		*(data+i) = buf[1];
		printf("Test Data %d ,buf[1] is 0x%x \n",*(data+i),buf[1]);		// Test Data 15 ,buf[1] is 0xf 

		#if 1
		for (tmp = 0; tmp < 4; tmp++) {
			printf("buf[%d]=0x%02x ", tmp, buf[tmp]);		// buf[0]=0x00 buf[1]=0x0f buf[2]=0x00 buf[3]=0x00 
		}

		printf("\n\n");
		#endif
	}

    if (ret != tr[0].len) {
        printf("SPI_IOC_MESSAGE error \n");
        ret = -1;
        goto end_spi_read;
    }
   ret=0;

end_spi_read:
	close(fd);
	return ret;
}

void icm_spi_operation()
{
	XAG_U32 fd = 0xac;
	uint8_t buf[0x10] = {0};

	/* open spi */
	//fd = icm_spi_open();

	/* config spi */
	//icm_spi_config(fd);

	while (1) {
		/* operation */

		// imu_spi_read_bmi088_id(fd);					// normal

		// user_spi_read(0x00|0x80, buf, 1);			// normal

		// user_spi_readGRY(0x00|0x80, buf, 1);			// normal

		// transfer();									// normal	

		sleep(1);
	}

}

int8_t user_spi_read(uint8_t reg_addr, uint8_t *data, uint16_t len)
{
	int retval = 0;
	int i = 0, index = 0;
	int tmp = 0;
	int fd = -1;
	char file_name[0x20];
	unsigned char buf[0x20] = {0};
	struct spi_ioc_transfer mesg[1];
	unsigned int spi_num = 6, csn = 1, cur_reg_addr; // dev_addr = 0, reg_addr = 0, cur_reg_addr = 0;
	unsigned int num_reg = 1, reg_width = 1, data_width = 1, reg_order = 1, data_order = 1; //dev_width = 1, 
    uint8_t tempValue[6];

	num_reg = len; 
	sprintf(file_name, "/dev/spidev%u.%u", 0, 0);

	fd = open(file_name, 0);
	if (fd < 0)
	{
		printf("Open %s error!\n", file_name);
		retval = -1;
		goto end0;
	}

	tmp = SPI_MODE_3;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval)
	{
		printf("set spi mode fail!\n");
		retval = -1;
		goto end1;
	}

	memset(mesg, 0, sizeof mesg);

	mesg[0].tx_buf = (__u64)buf;
	mesg[0].rx_buf = (__u64)buf;
	mesg[0].len = reg_width + data_width; 
	mesg[0].speed_hz = 5000000;  
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;

	memset(buf, 0, sizeof buf);
	memset(tempValue, 0, sizeof tempValue); //add 
  
	for (cur_reg_addr = reg_addr, i = 0; cur_reg_addr < reg_addr + num_reg; cur_reg_addr++, i++)
	{
		index = 0; //test
        
		if (reg_width == 1)
		{
			*(__u8 *)(&buf[index]) = cur_reg_addr & 0xff;
			index++;
		}
		else
		{
			if (reg_order)
				*(__u16 *)(&buf[index]) = cur_reg_addr & 0xffff;
			else
			{
				*(__u8 *)(&buf[index]) = (cur_reg_addr >> 8) & 0xff;
				*(__u8 *)(&buf[index + 1]) = cur_reg_addr & 0xff;
			}
			index += 2;
		}

		if (data_width == 1)
		{
			*(__u8 *)(&buf[index]) = 0x00;
		}
		else
		{
			*(__u16 *)(&buf[index]) = 0x0000;
		}

		//reverse8(buf, mesg[0].len);

		retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
		if (retval != (int)mesg[0].len)
		{
			printf("SPI_IOC_MESSAGE error \n");
			retval = -1;
			goto end1;
		}

		retval = 0;
		if (data_width == 1)
		{
			tmp = *(__u8 *)(&buf[index]);
			//*data= buf[index];	
			//*(data++) = buf[index];	//test
			printf("tmp ====== 0x%02x\n", tmp);	// 0x0f
			printf("buf:0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n", 
				buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);	// buf:0 f 0 0 0 0
			tempValue[i]=tmp;			
		}         
		else
		{
			if (data_order)
				tmp = *(__u16 *)(&buf[index]);
			else
			{
				tmp = *(__u8 *)(&buf[index]) << 8;
				tmp += (*(__u8 *)(&buf[index + 1]));
			}
		}
         
		 *data = tempValue[0]; //add
		 printf("data ====== 0x%02x\n", *data);	// 0xf
	}

   //copy multi bytes to DATA array!!
	if (i>2){
		   
			 for(int j=0;j<6;j++){
				 
				*(data+j) = tempValue[j];
				//printf("Test :data[%d] is  %d  ,tempValue[%d] is  %d \n", j,*(data+j) ,j,tempValue[j]);  //*data++ data[j]
			 }			
		 }

end1:
	close(fd);

end0:
	return retval;
}

/*
 * 对上面函数做了优化
 */
static XAG_S8 xag_spi_read_gyroscope(XAG_U8 reg_addr, XAG_U8 * data, XAG_U16 len)
{
	struct spi_ioc_transfer mesg[1];
	XAG_S32 retval = 0;
	XAG_S32 i = 0, index = 0;
	XAG_S32 tmp = 0;
	XAG_S32 fd = -1;
	XAG_S8 file_name[0x20] = { 0 };
	XAG_U8 buf[0x20] = { 0 };
	XAG_S32 cur_reg_addr; 
	XAG_S32 num_reg = 1, reg_width = 1, data_width = 1; 
    XAG_U8 temp_value[6] = { 0 };

	sprintf(file_name, "/dev/spidev%u.%u", 0, 0);

	fd = open(file_name, 0);
	if (fd < 0) {
		__do_abnormal("gyroscope spi open");
	}

	tmp = SPI_MODE_3;
	retval = ioctl(fd, SPI_IOC_WR_MODE, &tmp);
	if (retval) {
		__do_abnormal("spi wr mode");
	}

	memset(mesg, 0, sizeof mesg);
	mesg[0].tx_buf = (__u64)buf;
	mesg[0].rx_buf = (__u64)buf;
	mesg[0].len = reg_width + data_width; 
	mesg[0].speed_hz = 5000000;  
	mesg[0].bits_per_word = 8;
	mesg[0].cs_change = 1;

	memset(buf, 0, sizeof buf);
	memset(temp_value, 0, sizeof temp_value); 
  
	for (cur_reg_addr = reg_addr, i = 0; cur_reg_addr < reg_addr + len; cur_reg_addr++, i++) {
		index = 0; 
        
		if (reg_width == 1) {
			*(__u8 *)(&buf[index]) = (cur_reg_addr | 0x80) & 0xff;
			index++;
		}

		if (data_width == 1) {
			*(__u8 *)(&buf[index]) = 0x00;
		}

		retval = ioctl(fd, SPI_IOC_MESSAGE(1), mesg);
		if (retval != (XAG_S32)mesg[0].len) {
			printf("SPI_IOC_MESSAGE error \n");
			retval = -1;
			goto end1;
		}
		
		if (data_width == 1) {
			*(data+i) = *(__u8 *)(&buf[index]);
			// printf("asbuf:%x %x %x %x %x %x\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
			// printf("data:%x %x %x %x %x %x\n", data[0], data[1], data[2], data[3], data[4], data[5]);
			retval = 0;
		}
	}

end1:
	close(fd);

end0:
	return retval;
}